#include "pcontacts.h"

using namespace physics;

//////////////////////////////////////////////
// ParticleContact
//////////////////////////////////////////////
	//////////////////////////////////////////////
	// resolve()
	// 
	// resolves the velocity of the contact
	//////////////////////////////////////////////
	void ParticleContact::resolve(real dt)
	{
		resolveVelocity(dt);
		resolveInterpenetration(dt);
	}

	//////////////////////////////////////////////
	// calculateSeparatingVelocity()
	//
	// determines the separating velocity of the two
	// objects (if the second one exists - recall that
	// the second pointer could be NULL in the case where
	// an object is colliding with implied scenery such
	// as the ground)
	//////////////////////////////////////////////
	real ParticleContact::calculateSeparatingVelocity() const
	{
		Vector3 relativeVelocity = particle[0]->getVelocity();
		// if particle[1] exists, use its velocity to get
		// the relative velocity. Otherwise we just use the
		// velocity of the first object
		if (particle[1])
			relativeVelocity -= particle[1]->getVelocity();

		// return the dot product of the relative velocity and
		// the contact normal
		return relativeVelocity * contactNormal;
	}

	//////////////////////////////////////////////
	// resolveVelocity()
	//
	// determines if a collisions need to be resolved
	// and resolves them
	//////////////////////////////////////////////
	void ParticleContact::resolveVelocity(real dt)
	{
		// get the separating velocity
		real separatingVelocity = calculateSeparatingVelocity();

		// if it is greater than 0 (the objects are not moving toward 
		// one another, exit. Objects are either stationary
		// or separating already.
		if (separatingVelocity > 0)
			return;

		// the new separating velocity after resolution should
		// be modified by the coefficient of restitution
		real newSepVel = -separatingVelocity * restitution;

		// *** Special code to handle objects resting on the ground ***
		// check the velocity buildup due to acceleration
		Vector3 accCausedVelocity = particle[0]->getAcceleration();
		if (particle[1]) 
			accCausedVelocity -= particle[1]->getAcceleration();
		real accCausedSepVelocity = accCausedVelocity * contactNormal * dt;

		// if the objects are approaching one another due to the object's acceleration
		// we need to modify the separation velocity by this amount. If they are
		// still closing towards one-another, stop them altogether.
		if (accCausedSepVelocity < 0)
		{
			newSepVel += restitution * accCausedSepVelocity;

			if (newSepVel < 0) 
				newSepVel = 0;
		}
		

		// obtain the difference in velocity from the above calculation
		real deltaVel = newSepVel - separatingVelocity;

		// apply a change in velocity as a proprtion of its inverse mass
		real totalInverseMass = particle[0]->getInverseMass();
		if (particle[1]) 
			totalInverseMass += particle[1]->getInverseMass();

		// if both particles have an infinite mass, then exit
		if (totalInverseMass <= 0) 
			return;

		// calculate the impulse. Can't use mass here
		// to do a multiplication instead of a division
		// because one of the objects could have an
		// infinite mass, screwing up the math.
		real impulse = deltaVel / totalInverseMass;

		// the impulse per unit of IMass
		Vector3 impulsePerIMass = contactNormal * impulse;

		// apply the impulses to the particles
		// deltaVel is from the first object's frame 
		// of reference, so the second object requires
		// the minus sign
		particle[0]->setVelocity(particle[0]->getVelocity() + impulsePerIMass * particle[0]->getInverseMass());
		if (particle[1])
			particle[1]->setVelocity(particle[1]->getVelocity() - impulsePerIMass * particle[1]->getInverseMass());
	}
	//////////////////////////////////////////////
	// resolveInterpenetration()
	//
	// resolves the interpenetration of the two Particles
	// by moving them so they don't overlap anymore
	//////////////////////////////////////////////
	void ParticleContact::resolveInterpenetration(real dt)
	{
		// if there is no penetration, skip this function
		if (penetration <= 0)
			return;

		// get the total inverse mass
		real totalInverseMass = particle[0]->getInverseMass();
		if (particle[1]) 
			totalInverseMass += particle[1]->getInverseMass();

		// skip this function if both objects have
		// infinite mass
		if (totalInverseMass <= 0) 
			return;

		// the vector of penetration per IMass.
		// Can't use mass here for a multiplication, due to
		// the same reasons explained in resolveVelocity()
		Vector3 movePerIMass = contactNormal * (penetration / totalInverseMass);

		// calculate how far each object should move
		particleMovement[0] = movePerIMass * particle[0]->getInverseMass();
		if (particle[1])
			particleMovement[1] = movePerIMass * -particle[1]->getInverseMass();
		else
			particleMovement[1].clear(); // should be zero already, but just to be sure
		
		// apply the resolution to the penetration
		// by moving the objects the requisite amount
		particle[0]->setPosition(particle[0]->getPosition() + particleMovement[0]);
		if (particle[1])
			particle[1]->setPosition(particle[1]->getPosition() + particleMovement[1]);
	}

//////////////////////////////////////////////
// ParticleContactResolver
//////////////////////////////////////////////
	//////////////////////////////////////////////
	// ParticleContactResolver()
	// 
	// Initializes the contact resolver based on the iteration
	// limit passed in
	//////////////////////////////////////////////
	ParticleContactResolver::ParticleContactResolver(unsigned iterations)
	:
	iterationsAllowed(iterations)
	{
	}

	//////////////////////////////////////////////
	// setIterations()
	// 
	// sets the allowed number of iterations for
	// the contact resolver
	//////////////////////////////////////////////
	void ParticleContactResolver::setIterations(unsigned iterations)
	{
		ParticleContactResolver::iterationsAllowed = iterations;
	}

	//////////////////////////////////////////////
	// resolveContacts()
	// 
	// resolves the contacts stored in the passed
	// array
	//////////////////////////////////////////////
	void ParticleContactResolver::resolveContacts(ParticleContact *contactArray, unsigned numContacts, real dt)
	{
		unsigned i;
		iterationsUsed = 0;
		while (iterationsUsed < iterationsAllowed)
		{
			// resolve contacts by largest velocity first (the ones that will have
			// the biggest impact on everything else!

			// find the contact with the largest closing velocity
			real max = REAL_MAX;
			unsigned maxIndex = numContacts;
			for (i = 0; i < numContacts; i++)
			{
				// get the separating velocity
				real sepVel = contactArray[i].calculateSeparatingVelocity();
				// if this is the largest one yet (using less-than because these
				// are larger negative quantities for higher velocities) and it is
				// an actual collision (positive penetration or negative sepVel)
				// then save the information of the index and the sepVel
				if (sepVel < max && (sepVel < 0 || contactArray[i].penetration > 0))
				{
					max = sepVel;
					maxIndex = i;
				}
			}
			
			// if we didn't find anything to resolve, exit
			if (maxIndex == numContacts)
				break;

			// resolve the contact
			contactArray[maxIndex].resolve(dt);
			iterationsUsed++;
		}
	}